/*
 * Author  : Leo <lipf160627@163.com>
 * Created : 2025-7-27
 * Desc    : Ultrasonie driver
 */

#include "ultrasonic.hpp"

#include "esp_log.h"

static const char* TAG = "Ultrasonic";

Ultrasonic::Ultrasonic(gpio_num_t pinTrig, gpio_num_t pinEcho)
    : m_pinTrig(pinTrig), m_pinEcho(pinEcho), m_timeoutUs(30000)
{
    this->init();
}

Ultrasonic::~Ultrasonic() {
    gpio_reset_pin(m_pinTrig);
    gpio_reset_pin(m_pinEcho);
}

bool Ultrasonic::init() {
    // 配置Trig引脚为输出
    gpio_config_t io_conf = {};
    io_conf.pin_bit_mask = (1ULL << m_pinTrig);
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    esp_err_t ret = gpio_config(&io_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Trig GPIO config failed");
        return false;
    }

    // 配置Echo引脚为输入
    io_conf.pin_bit_mask = (1ULL << m_pinEcho);
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE; // 下拉确保稳定
    ret = gpio_config(&io_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Echo GPIO config failed");
        return false;
    }

    // 初始状态：Trig拉低
    gpio_set_level(m_pinTrig, 0);

    portMUX_INITIALIZE(&m_spinlock);

    ESP_LOGI(TAG, "Ultrasonic initialized: Trig=%d, Echo=%d", m_pinTrig, m_pinEcho);
    return true;
}

void Ultrasonic::delayUs(uint32_t us) {
    uint32_t start = esp_timer_get_time();
    while (esp_timer_get_time() - start < us) {
        // 忙等待
    }
}

uint64_t Ultrasonic::pulseIn() {
    // 等待Echo引脚变低（确保之前的状态已结束）
    uint32_t timeout = 0;
    while (gpio_get_level(m_pinEcho) == 1) {
        if (timeout++ > 1000) {
            return 0; // 超时
        }
        delayUs(10);
    }

    taskENTER_CRITICAL(&m_spinlock);

    // 发送10us的Trig脉冲
    gpio_set_level(m_pinTrig, 1);
    delayUs(10);
    gpio_set_level(m_pinTrig, 0);

    // 等待Echo引脚变高（脉冲开始）
    timeout = 0;
    while (gpio_get_level(m_pinEcho) == 0) {
        if (timeout++ > 1000) {
            return 0; // 超时
        }
        delayUs(10);
    }

    // 记录脉冲开始时间
    uint64_t startTime = esp_timer_get_time();
    
    // 等待Echo引脚变低（脉冲结束）
    timeout = 0;
    while (gpio_get_level(m_pinEcho) == 1) {
        if (esp_timer_get_time() - startTime > m_timeoutUs) {
            return 0; // 超时
        }
        if (timeout++ > 60000) { // 防止无限循环
            return 0;
        }
    }

    // 记录脉冲结束时间
    uint64_t endTime = esp_timer_get_time();

    taskEXIT_CRITICAL(&m_spinlock);

    return endTime - startTime;
}

float Ultrasonic::measureDistanceCM() {
    uint64_t duration = pulseIn();
    if (duration == 0) {
        return -1.0f; // 测量失败
    }

    // 距离 = (声速 × 时间) / 2
    // 声速 ≈ 340 m/s = 0.034 cm/μs
    float distance = (duration * 0.034) / 2.0f;
    
    // 有效范围：2cm - 400cm
    if (distance < 2.0f || distance > 400.0f) {
        return -1.0f;
    }

    return distance;
}
